// eee.Sheffield.PZ.Imaging
//
// Copyright ?Ping Zou, 2007
// sg71.cherub@gmail.com

using System;
using System.Collections.Generic;
using System.Text;
using System.IO;
using eee.Sheffield.PZ.Math;

namespace eee.Sheffield.PZ.Imaging.ImageFilter
{
    // Frame et al 1997
    public class FrameModel
    {
        // least square fit method (L-M)
        public void FrameLeastSquareFit(PZMath_vector sl,
            double init_a, double init_b, double init_mu, double init_sigma,
            out double a, out double b, out double mu, out double sigma)
        {
            PZMath_multifit_fdfsolver_type T = new PZMath_multifit_fdfsolver_type();
            PZMath_multifit_fdfsolver s = new PZMath_multifit_fdfsolver();

            int N = sl.Size;	// number of samples
            int status;
            int iter = 0;

            // prepare StateParam object
            int n = N;
            int p = 4;  // a, b, mu, sigma

            PZMath_vector y = new PZMath_vector(sl);	// vector of sample values
            double h = 1e-2; // step-size
            StateParam d = new StateParam(n, p, y, h, 1);

            // prepare model param
            double[] x_init = new double[p];	// initial parameter vaules
            x_init[0] = init_a;
            x_init[1] = init_b;
            x_init[2] = init_mu;
            x_init[3] = init_sigma;

            PZMath_vector x = new PZMath_vector(x_init);

            // prepare multifit solver
            PZMath_multifit_function_fdf f = new PZMath_multifit_function_fdf();
            PZMath_matrix covar = new PZMath_matrix(p, p);

            // allocate workspace
            f.f = new multifit_delegate_f(FrameModel.frame_fit_f);
            f.df = new multifit_delegate_df(FrameModel.frame_fit_df);
            f.fdf = new multifit_delegate_fdf(FrameModel.frame_fit_fdf);
            f.n = n;
            f.p = p;
            f.parms = d;
            T.Init("PZMath_multifit_fdfsolver_lmsder");
            s.Alloc(T, n, p);
            s.Init(f, x);

            s.PrintState(iter);

            // do multi parameter fitting
            do
            {
                iter++;
                status = s.Iterate();
                System.Diagnostics.Debug.WriteLine("status = " + status);
                s.PrintState(iter);

                if (status > 0)
                    break;

                status = s.TestDelta(s.dx, s.x, 1e-4, 1e-4);
            }
            while (status == PZMath_errno.PZMath_CONTINUE && iter < 500);

            // calculate covariance matrix
            s.Covar(0.0, covar);
            covar.DebugWriteLine();

            System.Console.WriteLine("a = " + s.FIT(0) + " +/- " + s.ERR(0, covar));
            System.Console.WriteLine("b = " + s.FIT(1) + " +/- " + s.ERR(1, covar));
            System.Console.WriteLine("mu = " + s.FIT(2) + " +/- " + s.ERR(2, covar));
            System.Console.WriteLine("sigma = " + s.FIT(3) + " +/- " + s.ERR(3, covar));

            double chi = s.Chi();
            System.Console.WriteLine("chisq/dof = " + (System.Math.Pow(chi, 2.0) / (double)(n - p)));
            System.Console.WriteLine("state = " + status);

            // save estimated parameters
            a = s.FIT(0);
            b = s.FIT(1);
            mu = s.FIT(2);
            sigma = s.FIT(3);
        } // FrameLeastSquareFit()

        static int frame_fit_f(PZMath_vector x, object param, PZMath_vector f)
        {
            // parse state data
            int n = (param as StateParam).n;
            PZMath_vector y = (param as StateParam).y;
            int samplerate = (param as StateParam).samplerate;

            // parse model parameters
            double a = x[0];
            double b = x[1];
            double mu = x[2];
            double sigma = x[3];

            // calculate the f(x)
            for (int i = 0; i < n; i++)
            // for each sample point
            {
                double t = (double)i / (double)samplerate;
                double k = a + (b - a) * System.Math.Exp(-1.0 * (t - mu) * (t - mu) / 2.0 / sigma / sigma);
                f[i] = k - y[i];
            }
            return PZMath_errno.PZMath_SUCCESS;
        } // frame_fit_f

        static int frame_fit_df(PZMath_vector x, object param, PZMath_matrix J)
        {
            // parse state data
            int n = (param as StateParam).n;
            PZMath_vector y = (param as StateParam).y;
            int samplerate = (param as StateParam).samplerate;

            // parse model parameters
            double a = x[0];
            double b = x[1];
            double mu = x[2];
            double sigma = x[3];

            // calculate Jacobian matrix
            for (int i = 0; i < n; i++)
            // for each sample point
            {
                // Jacobian matrix J(i, j) = dfi / dxj
                double t = (double)i / (double)samplerate;	// sample position
                double s = System.Math.Exp(-1.0 * (t - mu) * (t - mu) / 2.0 / sigma / sigma);
                J[i, 0] = 1 - s; // df/da
                J[i, 1] = s; // df/db
                J[i, 2] = (b - a) * (t - mu) * s / sigma / sigma; // df/dmu
                J[i, 3] = (b - a) * (t - mu) * (t - mu) * s / sigma / sigma / sigma; // df/dsigma
            }
            return PZMath_errno.PZMath_SUCCESS;
        } // frame_fit_df()

        static int frame_fit_fdf(PZMath_vector x, object param, PZMath_vector f, PZMath_matrix J)
        {
            frame_fit_f(x, param, f);
            frame_fit_df(x, param, J);
            return PZMath_errno.PZMath_SUCCESS;
        } // frame_fit_fdf()

        public int Frame(double initr, PZMath_vector sl,
            out double radius, out double centre, out double leftEdge, out double rightEdge)
        {
            int failCount = 0;

            // centre point comes as Sun 1987 matched filter
            double frame_estimateR = initr;
            SunMatchedFilter sunMatchedFilter = new SunMatchedFilter();
            PZMath_vector frame_output = sunMatchedFilter.RectangularMatchedFilter(sl, (int)frame_estimateR);
            // find the maximum output position
            int m = frame_output.MaxIndex();

            // match a Gaussian profile by least square fit (L-M)
            // k(x) = a + (b - a) * exp(- (x - mu) ^ 2 / 2 / sigma ^ 2)
            double frame_mean = sl.Mean;
            double frame_std = sl.StdDev;
            double init_a = frame_mean + frame_std;
            double init_b = frame_mean - frame_std;
            double init_mu = (double)m;
            double init_sigma = initr;
            double frame_a;
            double frame_b;
            double frame_mu;
            double frame_sigma;
            double frame_m;
            double frame_r;
            try
            {
                FrameLeastSquareFit(sl, init_a, init_b, init_mu, init_sigma,
                    out frame_a, out frame_b, out frame_mu, out frame_sigma);
                frame_m = frame_mu;
                frame_r = frame_sigma;
            }
            catch (ApplicationException e)
            {
                System.Diagnostics.Debug.WriteLine(e.Message);
                failCount++;
                frame_m = init_mu;
                frame_r = init_sigma;
            }

            PZMath_vector frame_edgePoints = new PZMath_vector(2);
            frame_edgePoints[0] = frame_m - frame_r;
            frame_edgePoints[1] = frame_m + frame_r;

            // output
            radius = frame_r;
            centre = frame_m;
            leftEdge = frame_edgePoints[0];
            rightEdge = frame_edgePoints[1];
            return failCount;
        } // Frame()

        public int FrameRecord(double initr, PZMath_vector sl, string fileName)
        {
            FileStream stream = new FileStream(fileName, FileMode.Create, FileAccess.Write);
            StreamWriter writer = new StreamWriter(stream);

            int failCount = 0;

            // centre point comes as Sun 1987 matched filter
            double frame_estimateR = initr;
            SunMatchedFilter sunMatchedFilter = new SunMatchedFilter();
            PZMath_vector frame_output = sunMatchedFilter.RectangularMatchedFilter(sl, (int)frame_estimateR);
            // find the maximum output position
            int m = frame_output.MaxIndex(); ;

            // match a Gaussian profile by least square fit (L-M)
            // k(x) = a + (b - a) * exp(- (x - mu) ^ 2 / 2 / sigma ^ 2)
            double frame_mean = sl.Mean;
            double frame_std = sl.StdDev;
            double init_a = frame_mean + frame_std;
            double init_b = frame_mean - frame_std;
            double init_mu = (double)m;
            double init_sigma = initr;
            double frame_a;
            double frame_b;
            double frame_mu;
            double frame_sigma;
            double frame_m;
            double frame_r;
            try
            {
                FrameLeastSquareFit(sl, init_a, init_b, init_mu, init_sigma,
                    out frame_a, out frame_b, out frame_mu, out frame_sigma);

                // plot fitted profile
                int width = sl.Size;
                PZMath_vector fittedProfile = new PZMath_vector(width);

                for (int i = 0; i < width; i++)
                {
                    fittedProfile[i] = frame_a + (frame_b - frame_a) *
                        System.Math.Exp(-1.0 * (i - frame_mu) * (i - frame_mu) / 2 / 2 / frame_sigma / frame_sigma);
                    writer.Write(String.Format("{0, -10:f}", fittedProfile[i]));
                }
                writer.WriteLine();

                frame_m = frame_mu;
                writer.WriteLine(frame_m);
                frame_r = frame_sigma;
                writer.WriteLine(frame_r);
            }
            catch (ApplicationException e)
            {
                System.Diagnostics.Debug.WriteLine(e.Message);
                failCount++;
                frame_m = init_mu;
                frame_r = init_sigma;
            }

            PZMath_vector frame_edgePoints = new PZMath_vector(2);
            frame_edgePoints[0] = frame_m - frame_r;
            frame_edgePoints[1] = frame_m + frame_r;

            writer.Flush();
            writer.Close();
            stream.Close();
            return failCount;
        } // FrameRecord()
    }
}
